//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
// 
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  $File: SimpleSoccerPlayer.cs $ $Revision: 1 $
//-----------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using referee = Microsoft.Robotics.Services.Simulation.SoccerReferee.Proxy;
using player = Microsoft.Robotics.Services.Simulation.SoccerPlayer.Proxy;
// use proxy versions of generic types so we get helper methods, etc
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;
using webcam = Microsoft.Robotics.Services.WebCam;
using W3C.Soap;
using System.Drawing;
using System.IO;
using Microsoft.Robotics.PhysicalModel;
using physicalmodelproxy = Microsoft.Robotics.PhysicalModel.Proxy;
using System.Drawing.Imaging;


namespace ProMRDS.SimpleSoccerPlayer
{

    /// <summary>
    /// The simple soccer player uses the generic player contract for all the type definitions
    /// </summary>
    [AlternateContract(player.Contract.Identifier)]
    [DisplayName("Better Soccer Player")]
    [Description("An improved version of the simple soccer player.")]
    [Contract(Contract.Identifier)]
    public class SimulatedSoccerPlayer : DsspServiceBase
    {

        [Partner(
            referee.Partners.Referee,
            Contract = referee.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExisting,
            Optional = false)]
        referee.SoccerRefereeOperations _refereePort = new referee.SoccerRefereeOperations();

        [InitialStatePartner(Optional = true)]
        SimpleSoccerPlayerState _state = new SimpleSoccerPlayerState();

        // Port for receiving generic player requests
        [AlternateServicePort(
            AllowMultipleInstances = true,
            AlternateContract = player.Contract.Identifier)]
        player.SoccerPlayerOperations _genericPlayerPort = new player.SoccerPlayerOperations();


        // Main service port for extended player behavior
        [ServicePort("/SimulatedSoccerPlayer", AllowMultipleInstances = true)]
        SimpleSoccerPlayerOperations _mainPort = new SimpleSoccerPlayerOperations();

        #region Init
        public SimulatedSoccerPlayer(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        protected override void Start()
        {
            base.Start();

            // we need to coordinate our own handlers that implement the Simple Player contract,
            // with the operations from the generic Player contract
            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(
                        Arbiter.Receive<DsspDefaultDrop>(false, _genericPlayerPort, DefaultDropHandler)
                    ),
                    new ExclusiveReceiverGroup
                    (
                        Arbiter.ReceiveWithIterator<player.Update>(true, _genericPlayerPort, PlayerUpdateHandler)
                    ),
                    new ConcurrentReceiverGroup(
                        Arbiter.Receive<DateTime>(true, _timerPort, GetFrame),
                        Arbiter.ReceiveWithIterator<player.Get>(true, _genericPlayerPort, PlayerGetHandler),
                        Arbiter.Receive<DsspDefaultLookup>(true, _genericPlayerPort, DefaultLookupHandler)
                    )
                )
            );

            ValidateState();
            SpawnIterator(AnnounceToReferee);
        }

        private void ValidateState()
        {
            if (_state == null)
            {
                _state = new SimpleSoccerPlayerState();
            }

            // we determine our role from either a config file or from our URI
            // URI suffix is easier so we just parse it instead of requiring different config files for 
            // each player role. Alternatively, teams can create different services all together, for different roles
            // (OffensiveSoccerPlayer, DefensiveSoccerPlayer, GoalKeeper, etc)

            if (ServiceInfo.Service.IndexOf("goalkeeper") != -1)
            {
                _state.Role = player.SoccerPlayerRole.GoalKeeper;
            }
            else
            {
                _state.Role = player.SoccerPlayerRole.Offense;
            }

            // similarly, look at the URI assigned to us to determine if this instance
            // of the SimplePlayer service should be affiliated with the blue or red team colors
            if (ServiceInfo.Service.IndexOf("redteam") != -1)
            {
                _state.Team = referee.TeamIdentifier.RedTeam;
            }
            else
            {
                _state.Team = referee.TeamIdentifier.BlueTeam;
            }

            _state.TeamName = "ProMRDS Soccer Team";

            if (_state.WebCamPollingIntervalInMs == 0)
            {
                _state.WebCamPollingIntervalInMs = 200;
            }

            _state.Mode = SoccerPlayerMode.FindBall;

            _webcamPollingInterval = TimeSpan.FromMilliseconds(_state.WebCamPollingIntervalInMs);
            SaveState(_state);           
        }

        // port for periodic timer messages used to drive webcam polling
        // and game logic
        Port<DateTime> _timerPort = new Port<DateTime>();

        drive.DriveOperations _robotDrive;
        webcam.WebCamOperations _robotCam;

        IEnumerator<ITask> AnnounceToReferee()
        {
            // ok we are to get our robotic services from the referee 
            // so we can start commanding the robot entity (or physical robot).
            // Send an announce message with our state

            referee.PlayerAnnounce announce = new referee.PlayerAnnounce();
            announce.Body = new referee.PlayerAnnounceRequest(
                AlternateContractServiceInfo[0].Service, // announce with generic player uri
                _state);
            _refereePort.Post(announce);

            referee.PlayerAnnounceResponse announceRsp = null;
            yield return Arbiter.Choice(announce.ResponsePort,
                delegate(referee.PlayerAnnounceResponse rsp)
                {
                    announceRsp = rsp;
                },
                delegate(Fault f) { LogError("Announce failed, shutting down", f); }
            );

            if (announceRsp == null)
            {
                Shutdown();
                yield break;
            }

            // we got the robotic services that can move the robot player and give
            // sensor feedback. Assign ports to them so we can send messages

            foreach (PartnerType pt in announceRsp.RoboticServices)
            {
                if (pt.Name.ToString() == referee.Partners.RobotMotionControl)
                {
                    // for this sample our motion control service is a simulated drive
                    _robotDrive = ServiceForwarder<drive.DriveOperations>(pt.Service + "/drive");
                    // enable robot drive
                    _robotDrive.EnableDrive(new drive.EnableDriveRequest(true));
                }
                else if (pt.Name.ToString() == referee.Partners.RobotCamera)
                {
                    _robotCam = ServiceForwarder<webcam.WebCamOperations>(pt.Service);
                }
            }

            // capture image request
            _queryFrameRequest.Format = Guid.Empty;
            _queryFrameRequest.Size.X = announceRsp.WebCamSize.X;
            _queryFrameRequest.Size.Y = announceRsp.WebCamSize.Y;

            // cause our time loop to start. We pull frames from
            // the web cam, at the pace we think is appropriate
            // If our state != Playing, then we dont really do any processing
            _timerPort.Post(DateTime.Now);


        }

        #endregion

        #region DSSP Handlers For Generic Player contract
        public IEnumerator<ITask> PlayerGetHandler(player.Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        public IEnumerator<ITask> PlayerUpdateHandler(player.Update update)
        {
            // update valid state. We drive the robot from our webcam
            // notifications, so the notification handler will take care of
            // doing the right thing, based on the current state
            _state.ValidState = update.Body.NewState;

            switch (_state.ValidState)
            {
                case referee.RobotState.Playing:
                    if (_robotDrive == null)
                    {
                        break;
                    }
                    // enable robot drive
                    _robotDrive.EnableDrive(new drive.EnableDriveRequest(true));
                    break;
                case referee.RobotState.Initial:
                case referee.RobotState.Ready:
                case referee.RobotState.Set:
                case referee.RobotState.Penalized:
                    _state.MostRecentCommand = SimpleSoccerPlayerCommand.Defend;
                    if (_robotDrive == null)
                    {
                        break;
                    }
                    // stop robot drive
                    _robotDrive.EnableDrive(new drive.EnableDriveRequest(false));
                    // set motor power to zero, just to be sure
                    _robotDrive.SetDrivePower(new drive.SetDrivePowerRequest(0, 0));
                    break;
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        #endregion

        #region DSSP Handlers for custom Simple Player Handlers

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        static TimeSpan ReissueFindCommandThreshold = TimeSpan.FromSeconds(5);
        static DateTime _lastCommandReceived = DateTime.MinValue;

        /// <summary>
        /// Executes commands from our internal logic or from other cooperating players
        /// </summary>
        /// <param name="command"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> ExecuteCommand(ExecuteCommand command)
        {

            try
            {
                bool alreadyExecutingCommand = _state.MostRecentCommand == command.Body.Command;

                switch (command.Body.Command)
                {
                    case SimpleSoccerPlayerCommand.ApproachBall:
                        if (_state.Role == player.SoccerPlayerRole.GoalKeeper)
                        {
                            // right now goalkeepers dont move
                            break;
                        }

                        _robotDrive.SetDrivePower(new drive.SetDrivePowerRequest(
                            (0.5f + _state.BallOffset) * 0.5f * 1.3,
                            (0.5f - _state.BallOffset) * 0.5f * 1.3));
                        break;
                    case SimpleSoccerPlayerCommand.Defend:
                        break;
                    case SimpleSoccerPlayerCommand.Kick:
                        SpawnIterator(KickBall);
                        break;
                    case SimpleSoccerPlayerCommand.FindBall:
                        if (_state.Role == player.SoccerPlayerRole.GoalKeeper)
                        {
                            break;
                        }

                        if (alreadyExecutingCommand)
                        {
                            if (DateTime.Now - _lastCommandReceived < ReissueFindCommandThreshold)
                            {
                                break;
                            }
                        }

                        _lastCommandReceived = DateTime.Now;

                        // randomly flip which way we rotate
                        float rotationDir = DateTime.Now.Ticks % 2 == 0 ? -1 : 1;

                        // spin around our axis, in order to find ball
                        _robotDrive.SetDrivePower(new drive.SetDrivePowerRequest(
                            0.3f * rotationDir,
                            -0.3f * rotationDir
                            ));
                        // we dont care about response, we hope it went through
                        break;
                    case SimpleSoccerPlayerCommand.Realign:
                        SpawnIterator(RealignApproach);
                        break;
                }
            }
            finally
            {
                command.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                _state.MostRecentCommand = command.Body.Command;
            }
            yield break;
        }

        IEnumerator<ITask> RealignApproach()
        {
            yield return Arbiter.Choice(
                _robotDrive.RotateDegrees(-45, 0.5),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault f) { }
            );

            yield return Arbiter.Choice(
                _robotDrive.DriveDistance(0.3, 0.5),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault f) { }
            );

            yield return Arbiter.Choice(
                _robotDrive.RotateDegrees(70, 0.5),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault f) { }
            );

            _state.Mode = SoccerPlayerMode.FindBall;
        }

        IEnumerator<ITask> KickBall()
        {
            yield return Arbiter.Choice(
                _robotDrive.DriveDistance(0.1, 0.8),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault f) { }
            );

            _state.Mode = SoccerPlayerMode.FindBall;
        }

        #endregion

        #region Game Logic

        TimeSpan _webcamPollingInterval;
        byte [] _lastFrame;
        webcam.QueryFrameRequest _queryFrameRequest = new webcam.QueryFrameRequest();

        void StartWebCamTimer()
        {
            TaskQueue.EnqueueTimer(
                    TimeSpan.FromMilliseconds(_state.WebCamPollingIntervalInMs),
                    _timerPort);
        }
        
        /// <summary>
        /// The GetFrame method is called N times a second to process a webcam
        /// Image and determine the relative position of the ball and opponent goal
        /// This routine is just a sample, by no means the right way to play soccer/
        /// It has simple, sequential logic, and all it really does is approach the ball
        /// Boundary detection and opponent bot detection is missing.
        /// Note that bumper notifications can be processed concurrently with this
        /// and override behavior if collision is detected
        /// </summary>
        /// <returns></returns>
        private void GetFrame(DateTime timestamp)
        {
            // unless we are in the playing state, dont do anything
            if (_state.ValidState != referee.RobotState.Playing)
            {
                StartWebCamTimer();
                return;
            }

            webcam.QueryFrame query = new webcam.QueryFrame();
            query.Body = _queryFrameRequest;
            _robotCam.Post(query);
            // retrieve a frame from the camera, analyze, the send motor commands
            Activate(Arbiter.Choice(
                query.ResponsePort,
                delegate(webcam.QueryFrameResponse rsp)
                {
                    _lastFrame = rsp.Frame;
                    // tell our webcam to update its frame
                    _robotCam.Post(new webcam.UpdateFrame());

                    // now run some internal recognition to determine if we see the ball
                    // and if we do, where to go.
                    // Also determine where the opponent goal is.
                    SpawnIterator(ProcessFrameAndDetermineBehavior);
                },
                delegate(Fault f)
                {
                    _lastFrame.Initialize();
                    LogError(LogGroups.Console, "Could not query webcam frame", f);
                    StartWebCamTimer();
                }));
        }

        ImageSegment [] _segments = new ImageSegment[3];
        int _previousArea = 0;

        IEnumerator<ITask> ProcessFrameAndDetermineBehavior()
        {

            if ((_state.Mode == SoccerPlayerMode.ReAlign) ||
                (_state.Mode == SoccerPlayerMode.Kick) ||
                (_state.Role == Microsoft.Robotics.Services.Simulation.SoccerPlayer.Proxy.SoccerPlayerRole.GoalKeeper))
            {
                StartWebCamTimer();
                yield break;    // realign or shoot in progress, don't do anything until it is finished
            }

            try
            {
                // make sure the segments are initialized
                if (_segments[0] == null)
                {
                    _segments[0] = new ImageSegment();
                    _segments[0].Hue = 0.1f;
                    _segments[0].HueEpsilon = 0.1f;
                    _segments[0].Saturation = 0.75f;
                    _segments[0].SaturationEpsilon = 0.5f;
                    _segments[1] = new ImageSegment();
                    _segments[1].Hue = 180f;
                    _segments[1].HueEpsilon = 5f;
                    _segments[1].Saturation = 0.75f;
                    _segments[1].SaturationEpsilon = 0.5f;
                    _segments[2] = new ImageSegment();
                    _segments[2].Hue = 60f;
                    _segments[2].HueEpsilon = 5f;
                    _segments[2].Saturation = 0.75f;
                    _segments[2].SaturationEpsilon = 0.5f;
                }

                try
                {
                    ImageProcessing.ProcessFrameHandler(
                        _queryFrameRequest.Size.X,
                        _queryFrameRequest.Size.Y,
                        _segments,
                        _lastFrame);
                }
                catch (Exception ex)
                {
                    LogError(ex);
                }

                // if ProcessFrameHandler fails, use the previous results
                bool foundBall = _segments[0].Area > 10;
                bool foundBlueGoal = (_segments[1].Area > 500) && (_segments[1].XSpan > 75);
                bool foundYellowGoal = _segments[2].Area > 500 && (_segments[1].XSpan > 75);

                // uncomment for debugging
                //if ((_state.Team == );.TeamIdentifier.BlueTeam) &&
                //    (_state.Role == Microsoft.Robotics.Services.Simulation.SoccerPlayer.Proxy.SoccerPlayerRole.Offense))
                //{
                //    string output = " ";
                //    if (foundBall)
                //        output += "Ball(" + _segments[0].X + ")" + "  " + _segments[0].Area + " ";

                //    if (foundBlueGoal)
                //        output += "Blue(" + _segments[1].X + ")" + "  " + _segments[1].Area + " ";

                //    if (foundYellowGoal)
                //        output += "Yellow(" + _segments[2].X + ")" + "  " + _segments[2].Area + " ";

                //    Console.WriteLine(output);
                //}

                switch (_state.Mode)
                {
                    case SoccerPlayerMode.FindBall:
                    case SoccerPlayerMode.Approach:
                        {
                            if (!foundBall)
                            {
                                if (_previousArea > 4000)
                                {
                                    // we are right on top of the ball so we can't see it.  Kick!
                                    _previousArea = 0;
                                    _mainPort.Post(new ExecuteCommand(new ExecuteCommandRequest(SimpleSoccerPlayerCommand.Kick)));
                                }
                                else
                                {
                                    _mainPort.Post(new ExecuteCommand(new ExecuteCommandRequest(SimpleSoccerPlayerCommand.FindBall)));
                                    _previousArea = 0;
                                }
                            }
                            else
                            {
                                if (((_state.Team == referee.TeamIdentifier.BlueTeam) && foundBlueGoal) ||
                                    (_state.Team == referee.TeamIdentifier.RedTeam && foundYellowGoal) ||
                                    (_segments[0].Area < 2000) || (_segments[0].Area > 4000))
                                {
                                    // either the goal is lined up for a shot or the ball is too far away or to close to decide
                                    _state.Mode = SoccerPlayerMode.Approach;
                                    _previousArea = (int)_segments[0].Area;

                                    // calculate an offset from center that will be used to calculate left/right drive power
                                    _state.BallOffset = 0.5f * (_segments[0].X - 0.5f * _queryFrameRequest.Size.X) / _queryFrameRequest.Size.X;
                                    // we found the ball, calculate a relative 3d vector towards it
                                    _mainPort.Post(new ExecuteCommand(new ExecuteCommandRequest(SimpleSoccerPlayerCommand.ApproachBall)));
                                }
                                else
                                {
                                    _state.Mode = SoccerPlayerMode.ReAlign;
                                    // we found the ball but we're not lined up properly for a shot, realign
                                    _mainPort.Post(new ExecuteCommand(new ExecuteCommandRequest(SimpleSoccerPlayerCommand.Realign)));
                                }
                            }
                            break;
                        }
                }

            }
            finally
            {
                // start timer again which will cause our GetFrame handler to be called
                // Note that we activated a persisted receiver, once, on the timerPort already, on startup
                StartWebCamTimer();
            }
            yield break;
        }

        #endregion
    }
}
